/*  Ce programme effectue l'asservissement en rotation du robot */

#include <Wire.h>
#include <SoftwareSerial.h>


/****  Définition des adresses des registres de la carte moteur  ****/
#define MD25ADDRESS 0x5F // Adresse de la carte moteur
#define SPEED1              (byte)0x00  // Adresse où est stockée la vitesse du moteur 1
#define SPEED2              0x01        // Adresse où est stockée la vitesse du moteur 2
#define ENCODER1            0x02        // Adresse du registre contenat la valeur de l'encodeur du moteur 1
#define ENCODER2            0x06  // Adresse du registre contenant la valeur de l'encodeur du moteur 2
#define CURRENT1            0xB   // Adresse du registre contenant la valeur de l'intensité du courant du moteur 1
#define CURRENT2            0xC   // Adresse du registre contenant la valeur de l'intensité du courant du moteur 2
#define CMD                 0x10
#define ACCELERATION_RATE   0x0E  // Adresse du registre contrôlant l'accélération du robot

/****Variables globales ****/
long encoder1;// variable dans laquelle sera stockée la valeur de l'encodeur du moteur 1
long encoder2;// variable dans laquelle sera stockée la valeur de l'encodeur du moteur 2
byte current1;// variable dans laquelle sera stockée la valeur du courant dans le moteur 1
byte current2;// variable dans laquelle sera stockée la valeur du courant dans le moteur 2
long orientation_current; // Orientation du robot issue de la dernière mesure
long orientation_old;  // Orientation du robot issue de l'avant dernière mesure
float Kp = 0.1;//Coefficient de la commande proportionnelle : 
float Kd = 0;// Coefficient de la commande dérivée// 0.1 si acceleration = 0.5
int commande;// commande calculée lors de l'asservissement
unsigned long time_begin;// Date du début de la période d'échantillonnage en cours

unsigned long sampling_period = 200; // Periode d'échantillonage en millisecondes  //!\\ Si elle trop grande ou petite, l'asservissement devient instable
long waitingTime;// Temps d'attente de la fin de la période d'échantillonnage 
int  count ;// Pour l'asservissement 


void setup(){
  // Initialisation de la communication série avec l'ordinateur
  Serial.begin(9600);
  //Initialisation de la communication I2C (protocole pour communiquer avec la carte moteur)
  Wire.begin();
  // Moteurs à l'arret
  setSpeed1(128);
  setSpeed2(128);
  //Initialisation des encodeurs
  resetEncoders();
  delay(2000);// Attente de 2 secondes
  setAccelerationRate(10);// Réglage de l'accélération max (1 : mini; max : maxi)
  getAccelerationRate();
  encoder1 = getEncoder1();
  encoder2 = getEncoder2();
  Serial.println(encoder1);
  Serial.println(encoder2);
  //Initialisation de l'orientation du robot 
  orientation_old = 0;
  orientation_current = 0;
  Serial.print("Commande proportionnelle : ");
  Serial.println(Kp);
  Serial.println("Initialisation terminée");
  delay(2000);
  count = 0;
  
}



void loop(){
  /* Tourne de 90° */
  tourne(458);
  Serial.println("Fini");
  delay(5000);
  /* Tourne de 90° */
  tourne(2*458);
  delay(5000);
  /* Tourne encore de 90° */
  tourne(3*458);
  delay(2000);
  /* Tourne encore de 90° */
  tourne(4*458);
  delay(5000);

}


// Consigne en rotation du robot : x=458 <--> tourne de 90°
void tourne(long x){
  Serial.print("Count ");
  Serial.println(count);

  time_begin = millis();

  // Début de la boucle d'asservissement 
  // count : compteur qui permet de détecter quand le robot est stabilisé et donc que l'asservissement est termniné
  // Si count < 5, alors on continue l'asservissement,sinon on sort de la boucle while
  while( count <= 5){

    /* Si l'erreur en orientation par rapport à la consigne est inférieure à une certaine valeur, on incrémente count */
    if ((getEncoder1() - getEncoder2() - x) <=10 && (getEncoder1() - getEncoder2() - x) >= -10){
      count++;
      Serial.println("Incrément");
    } 

    Serial.print("L'Erreur est de :  ");
    Serial.println(getEncoder1() - getEncoder2() - x);
    
   
    orientation_old = orientation_current;// la dernière mesure de l'orientation devient l'ancienne mesure

    // Si la période d'échantillonnage est trop courte... 
    if (waitingTime = ( sampling_period - (millis() - time_begin) ) < 0){
      Serial.println("Erreur : la boucle dure plus longtemps que la période d'échantillonnage : il faut augmenter la période d'échantillonnage-------------------------------------------------");
    }
    
    // On attend la fin de la période d'échantillonnage
    delay(waitingTime = ( sampling_period - (millis() - time_begin) ));
    Serial.print("Temps d'attente  " );
    Serial.println(waitingTime);

    
    // Début de la nouvelle période d'échantillonnage
    time_begin = millis();
    // La fonction "rotate" va calculer la commande à envoyer aux moteurs
    asservissement_rotation(x);

  }
  // L'asservissement est terminé, on remet le compteur à zéro
  count = 0;

}



// Fonction appelée par la fonction "tourne"
void asservissement_rotation(int x){// Fait tourner le robot : x=458 <--> 90°

  /* Calcul de l'orientation actuelle du robot à partir de la mesure des encodeurs des roues */
  Serial.print("Encodeur 1 : ");
  Serial.println(getEncoder1());

  Serial.print("Encodeur 2 : ");
  Serial.println(getEncoder2());
  
  // L'orientation est :orientation_current
  orientation_current = getEncoder1() - getEncoder2();
  Serial.print("Orientation : ");
  Serial.println(orientation_current);

  //Calcul de la commande à partir de cette mesure
  commande =  Kp*(x - orientation_current) + Kd*(orientation_old - orientation_current) ; // J'ai changé le signe!!
  Serial.print("Commande : ");
  Serial.println(commande);
  Serial.println(" ");
  
  if (commande <= 127 && commande >= -127 ){
    setSpeed1(commande + 128);
    setSpeed2(-commande + 128);
  }
  else if (commande >= 128){ // Saturation de la vitesse
    setSpeed1(255);
    setSpeed2(0);
  }
  else if(commande <= -128){ // Saturation de la vitesse
    setSpeed1(0);
    setSpeed2(255);
  }


}


// Garde fou : arrête les moteurs si le courant dans les moteurs est supérieur à x
void watchCurrents(float x){
  if(getCurrent1() > x || getCurrent2() > x ){
    setSpeed1(128);
    setSpeed2(128);
    Serial.println("intensité trop grande !--------------------------------------------------------------------------------------------");
  }
}

// Commande de vitesse du moteur 1 : x =128 : arrêt ; x = 0: marche arrière max; x =255 : marche avant max
void setSpeed1(int x){
  // Début de la communication avec la carte moteur
  Wire.beginTransmission(MD25ADDRESS);
  // Accès au registre stockant la vitesse du moteur 1
  Wire.write(SPEED1);
  // Ecriture dans le registre de la consigne en vitesse
  Wire.write(x);   
  // Fin de la communication
  Wire.endTransmission();
}

// Commande de vitesse du moteur 2
void setSpeed2(int x){
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(SPEED2);
  Wire.write(x);
  Wire.endTransmission();
}


long getEncoder1(){ // Lecture de l'encodeur du moteur 1
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(ENCODER1);
  Wire.endTransmission();

  Wire.requestFrom(MD25ADDRESS,4);
  while(Wire.available() < 4);// l'encodeur code sur 32 bits. Il faut donc attendre que la carte moteur renvoit 4 bytes
  long encoder1 = Wire.read(); // Reception des 8 bits de poids les plus forts
  encoder1 <<= 8;
  encoder1 += Wire.read();
  encoder1 <<= 8;
  encoder1 += Wire.read();
  encoder1 <<= 8;
  encoder1 += Wire.read();// Reception des 8 bits de poids les plus faibles

  return(encoder1);
}


long getEncoder2(){ // Lecture de l'encodeur du moteur 2
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(ENCODER2);
  Wire.endTransmission();

  Wire.requestFrom(MD25ADDRESS,4);
  while(Wire.available() < 4);
  long encoder2 = Wire.read();
  encoder2 <<= 8;
  encoder2 += Wire.read();
  encoder2 <<= 8;
  encoder2 += Wire.read();
  encoder2 <<= 8;
  encoder2 += Wire.read();

  return(encoder2);




}
void resetEncoders(){// Met à 0 les encodeurs
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(CMD);
  Wire.write(0x20);
  Wire.endTransmission();

}

byte getCurrent1(){ // Lit le courant dans le moteur 1 : la valeur retournée est égale à 10 fois l'intensite : 10 <--> 1A
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(CURRENT1);
  Wire.endTransmission();

  Wire.requestFrom(MD25ADDRESS,1);
  while(Wire.available() < 1);
  byte current1 = Wire.read();

  return(current1);
}

byte getCurrent2(){ // Lit le courant dans le moterur 2 : la valeur retournée est égale à 10 fois l'intensite : 10 <--> 1A
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(CURRENT2);
  Wire.endTransmission();

  Wire.requestFrom(MD25ADDRESS,1);
  while(Wire.available() < 1);
  byte current2 = Wire.read();

  return(current2);

}

void setAccelerationRate(int x){
  if(x >=1 && x <=10){
    Wire.beginTransmission(MD25ADDRESS);
    Wire.write(ACCELERATION_RATE);
    Wire.write(x);
    Wire.endTransmission();
  }
  else{
    Serial.println("Valeur rentrée incorrecte pour le taux d'accélération");
  }
}

void getAccelerationRate(){// Affiche à l'écran le taux d'accélération : 1(minimale) à 10 (maximal)
  Wire.beginTransmission(MD25ADDRESS);
  Wire.write(ACCELERATION_RATE);
  Wire.endTransmission();
  Wire.requestFrom(MD25ADDRESS,1); 
  while(Wire.available() < 1); 
  byte acceleration = Wire.read();
  Serial.println(acceleration);
}









